/*
 * Copyright (c) 2024 iSOFT INFRASTRUCTURE SOFTWARE CO., LTD.
 * easyAda is licensed under Mulan PubL v2.
 * You can use this software according to the terms and conditions of the Mulan PubL v2.
 * You may obtain a copy of Mulan PubL v2 at:
 *          http://license.coscl.org.cn/MulanPubL-2.0
 * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
 * EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
 * MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
 * See the Mulan PubL v2 for more details.
 */

#include <plat/config.h>

.macro  switch_el, xreg, el3_label, el2_label, el1_label
    mrs \xreg, CurrentEL
    cmp \xreg, 0xc
    b.eq \el3_label
    cmp \xreg, 0x8
    b.eq \el2_label
    cmp \xreg, 0x4
    b.eq \el1_label
.endm

.macro save_context_and_set_exception_stack
    stp x29, x30, [sp, #-16]!	
    stp x27, x28, [sp, #-16]!
    stp x25, x26, [sp, #-16]!
    stp x23, x24, [sp, #-16]!
    stp x21, x22, [sp, #-16]!
    stp x19, x20, [sp, #-16]!
    stp x17, x18, [sp, #-16]!
    stp x15, x16, [sp, #-16]!
    stp x13, x14, [sp, #-16]!
    stp x11, x12, [sp, #-16]!
    stp x9, x10, [sp, #-16]!
    stp x7, x8, [sp, #-16]!
    stp x5, x6, [sp, #-16]!
    stp x3, x4, [sp, #-16]!
    stp x1, x2, [sp, #-16]!
    # mrs x0,sp_el1
    mrs x1,elr_el1
    mrs x2,sp_el0
    # add x3,sp,#(30 * 8)
    mrs x3, spsr_el1
    stp x1, x0, [sp, #-16]!
    stp x3, x2, [sp, #-16]!
    mov x0,sp
    bl setup_temp_stack
.endm

.globl vectors_entry
.section ".vector","ax"
.align 11
vectors_entry:

    .align 7                /* synchronous offset = 0x000 */
    save_context_and_set_exception_stack
    bl _exception_entry
    bl handle_sync_entry
    b exception_exit

    .align 7                /*IRQ/vIRQ offset = 0x080 */
    stp x29, x30, [sp, #-16]!
    save_context_and_set_exception_stack
    bl _exception_entry
    bl handle_irq_entry
    b exception_exit

    .align 7                /* FIQ/vFIQ offset = 0x100 */
    stp x29, x30, [sp, #-16]!
    save_context_and_set_exception_stack
    bl _exception_entry
    bl handle_fiq_entry
    b exception_exit

    .align 7                /* SError/SError offset = 0x180 */
    stp x29, x30, [sp, #-16]!
    save_context_and_set_exception_stack
    bl _exception_entry
    bl handle_serror_entry
    b exception_exit


    .align 7                /* synchronous offset = 0x200 */
    bl setup_temp_stack
    bl _exception_entry
    bl handle_sync_entry
    b exception_exit

    .align 7                /*IRQ/vIRQ offset = 0x280 */
    save_context_and_set_exception_stack
    bl handle_irq_entry

    .align 7                 /* FIQ/vFIQ offset = 0x300  */
    bl setup_temp_stack
    bl _exception_entry
    bl handle_fiq_entry
    b exception_exit

    .align 7                 /* SError/SError offset = 0x380 */
    bl setup_temp_stack
    bl _exception_entry
    bl handle_serror_entry
    b exception_exit


    .align 7                /* synchronous offset = 0x400 */
    save_context_and_set_exception_stack

    mrs x25,esr_el1
    lsr x24,x25,#26
    cmp x24,#0x15
    b.eq syscall_handler
    mov sp,x0
    add sp,sp,#(32*8)
    bl _exception_entry 
    bl handle_sync_entry
    b exception_exit

    .align 7                /* IRQ/vIRQ offset = 0x480 */
    save_context_and_set_exception_stack
    bl handle_irq_entry

    .align 7                /* FIQ/vFIQ offset = 0x500  */
    save_context_and_set_exception_stack
    bl _exception_entry
    bl handle_fiq_entry
    b exception_exit

    .align 7                /* SError/SError offset = 0x580 */
    save_context_and_set_exception_stack
    bl _exception_entry
    bl handle_serror_entry
    b exception_exit


    .align 7                /* synchronous offset = 0x600 */
    save_context_and_set_exception_stack
    bl _exception_entry
    bl handle_sync_entry
    b exception_exit

    .align 7                /* IRQ/vIRQ offset = 0x680 */
    save_context_and_set_exception_stack
    bl _exception_entry
    bl handle_irq_entry
    b  exception_exit

    .align 7                /* FIQ/vFIQ offset = 0x700  */
    save_context_and_set_exception_stack
    bl _exception_entry
    bl handle_fiq_entry
    b exception_exit

    .align 7                /* SError/SError offset = 0x780 */
    save_context_and_set_exception_stack
    bl _exception_entry
    bl handle_serror_entry
    b exception_exit


_exception_entry:
    stp x27, x28, [sp, #-16]!
    stp x25, x26, [sp, #-16]!
    stp x23, x24, [sp, #-16]!
    stp x21, x22, [sp, #-16]!
    stp x19, x20, [sp, #-16]!
    stp x17, x18, [sp, #-16]!
    stp x15, x16, [sp, #-16]!
    stp x13, x14, [sp, #-16]!
    stp x11, x12, [sp, #-16]!
    stp x9, x10, [sp, #-16]!
    stp x7, x8, [sp, #-16]!
    stp x5, x6, [sp, #-16]!
    stp x3, x4, [sp, #-16]!
    stp x1, x2, [sp, #-16]!

    switch_el x1, 3f, 2f, 1f
3:
    mrs x1, esr_el3
    mrs x2, elr_el3
    mrs x3, daif
    mrs x4, vbar_el3
    mrs x5, spsr_el3
    add x6, sp, #(8*30)
    mrs x7, sctlr_el3
    mrs x8, scr_el3
    mrs x9, ttbr0_el3
    b 0f
2:  
    mrs x1, esr_el2
    mrs x2, elr_el2
    mrs x3, daif
    mrs x4, vbar_el2
    mrs x5, spsr_el2
    add x6, sp, #(8*30)
    mrs x7, sctlr_el2
    mrs x8, hcr_el2
    mrs x9, ttbr0_el2
    b 0f
1:
    mrs x1, esr_el1
    mrs x2, elr_el1
    mrs x3, daif
    mrs x4, vbar_el1
    mrs x5, spsr_el1
    add x6, sp, #(8*30)
    mrs x7, sctlr_el1
    mrs x8, sp_el0
    mrs x9, ttbr0_el1
0:
    stp x2, x0, [sp, #-16]!
    stp x3, x1, [sp, #-16]!
    stp x5, x4, [sp, #-16]!
    stp x7, x6, [sp, #-16]!
    stp x9, x8, [sp, #-16]!
    mov x0, sp
    ret 

exception_exit:
    add sp, sp, #(8*8)/* see: sys registers size of struct pt_regs */
    ldp x2, x0, [sp],#16		
    switch_el x11, 3f, 2f, 1f
3:  msr elr_el3, x2
    b 0f
2:  msr elr_el2, x2
    b 0f
1:  msr elr_el1, x2
0:
    ldp x1, x2, [sp],#16
    ldp x3, x4, [sp],#16
    ldp x5, x6, [sp],#16
    ldp x7, x8, [sp],#16
    ldp x9, x10, [sp],#16
    ldp x11, x12, [sp],#16
    ldp x13, x14, [sp],#16
    ldp x15, x16, [sp],#16
    ldp x17, x18, [sp],#16
    ldp x19, x20, [sp],#16
    ldp x21, x22, [sp],#16
    ldp x23, x24, [sp],#16
    ldp x25, x26, [sp],#16
    ldp x27, x28, [sp],#16
    ldp x29, x30, [sp],#16
    eret


setup_temp_stack:
#ifdef CONFIG_ENABLE_SMP
    mrs x5, mpidr_el1
    mov x6, #0xff00
    and x5, x5, x6 
    lsr x5, x5, #8
    mov x6, #(2 * CONFIG_STACK_SIZE)
    ldr x7, =temp_stack
    add x5, x5, #1
    mul x6, x6, x5
    add x7, x7, x6
    mov sp, x7
#else
    ldr x7, =temp_stack
    add x7, x7,#(2 * CONFIG_STACK_SIZE)
    mov sp, x7
#endif
    ret


.section ".stack"

temp_stack:
#ifdef CONFIG_ENABLE_SMP
    .space (CONFIG_SMP_CORES * CONFIG_STACK_SIZE)
#else
    .space (2 * CONFIG_STACK_SIZE)
#endif




